import rclpy
from rclpy.node import Node
from chapt4_interfaces.srv import FaceDetector
import face_recognition
import cv2
from ament_index_python.packages import get_package_share_directory
import os
from cv_bridge import CvBridge
import time
from rcl_interfaces.msg import SetParametersResult

class FaceDetectNode(Node):
    def __init__(self):
        super().__init__("face_detect_node")
        self.get_logger().info("人脸检测服务启动...")
        self.service_ = self.create_service(
            FaceDetector,  #消息接口，就是参数
            "face_detect", #服务名称
            self.detect_face_callback  #回调函数
            )
        self.bridge_ = CvBridge()

        #参数声明
        self.declare_parameter('number_of_times_to_upsample',1)
        self.declare_parameter('model','hog') 
        #参数应用
        self.number_of_times_to_upsample=self.get_parameter('number_of_times_to_upsample').value
        self.model=self.get_parameter('model').value
        #self.number_of_times_to_upsample=1
        #self.model="hog"

        self.defalut_img_path = get_package_share_directory('demo_python_service') + '/resource/default.jpg'
        self.get_logger().info(f"人脸检测服务已启动...")
        self.add_on_set_parameters_callback(self.parameter_callback)
        # #设置自身节点参数的方法,和上面的 参数声明+参数应用 是一回事
        # self.set_parameters([rclpy.Parameter('model',rclpy.Parameter.Type.STRING,'hog')])


    #参数回调函数,当外部在 调用  ros2 param set /face_detect_node number_of_times_to_upsample 2 时，会自动调用这个函数 
    # 当然也有内置的service 接口 来修改 这些参数，如 ros2 interface show rcl_interfaces/srv/SetParameters
    def parameter_callback(self,parameters):
        for param in parameters:
            self.get_logger().info(f"{param.name}参数 value：{param.value}")
            if param.name == 'number_of_times_to_upsample':
                self.number_of_times_to_upsample = param.value
                self.get_logger().info(f"number_of_times_to_upsample参数已更新为：{self.number_of_times_to_upsample}")
            elif param.name == 'model':
                self.model = param.value 
                self.get_logger().info(f"model参数已更新为：{self.model}")
        return SetParametersResult(successful=True)        

    def detect_face_callback(self,request,response):
        #request.image 这里的image 是FaceDetector.srv 格式里的image  
        if request.image.data:  
            # 将ROS2的图像消息转换为OpenCV的图像格式
            cv_image = self.bridge_.imgmsg_to_cv2(request.image)
        else:
            cv_image = cv2.imread(self.defalut_img_path)  
            self.get_logger().info(f"传入图像为空，使用默认图像!...")
        #cv_image 已经是一个opencv格式的图像了
        start_time = time.time();
        self.get_logger().info(f"加载完成图像，开始识别!...")
        # 检测人脸
        face_locations = face_recognition.face_locations(
            cv_image,
            number_of_times_to_upsample=self.number_of_times_to_upsample,
            model=self.model
            )
        end_time = time.time();
        response.use_time = end_time - start_time
        response.number = len(face_locations)

        for top,right,bottom,left in face_locations:
            response.top.append(top)
            response.right.append(right)
            response.bottom.append(bottom)
            response.left.append(left)

        return response #必须返回response
        


def main():
    rclpy.init()
    node = FaceDetectNode()
    rclpy.spin(node)
    rclpy.shutdown()